#include "../Device/control.h"

#include "../Task/imu.h"
#include "sys.h"

extern TIM_HandleTypeDef htim2;
float angle_setpoint, speed_setpoint, speed_max;
float angle_p, angle_i, angle_d, speed_p, speed_i, speed_d, turn_p;

/**
 * @brief 电机控制主程序
 * @param argument: 未使用
 * @retval 无
 */
void Motor_Control(void *argument) {
  const uint32_t delay_tick = osKernelGetTickFreq() / CONTROL_FREQ;

  Control_Init();

  uint32_t tick = osKernelGetTickCount();
  while (1) {
    tick += delay_tick;

    Control_Apply();

    osDelayUntil(tick);
  }
}
